#include "camera_thread.h"
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <sys/mman.h>
#include <string.h>
#include <errno.h>
#include <QDebug>

#define CLEAR(x) memset(&(x), 0, sizeof(x))

CameraThread::CameraThread(QObject *parent) : QThread(parent) {}

CameraThread::~CameraThread() {
    stopCapture();
}

void CameraThread::setDevice(const QString &device) {
    m_device = device;
}

void CameraThread::stopCapture() {
    m_running = false;
    wait();
}

bool CameraThread::initCamera() {
    // 打开设备
    m_v4l2Fd = open(m_device.toUtf8().constData(), O_RDWR | O_NONBLOCK);
    if (m_v4l2Fd < 0) {
        emit errorOccurred(QString("Cannot open camera: %1").arg(strerror(errno)));
        return false;
    }

    // 查询设备能力
    struct v4l2_capability cap;
    CLEAR(cap);
    if (ioctl(m_v4l2Fd, VIDIOC_QUERYCAP, &cap) < 0) {
        emit errorOccurred(QString("VIDIOC_QUERYCAP failed: %1").arg(strerror(errno)));
        return false;
    }

    if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
        emit errorOccurred("Device does not support video capture");
        return false;
    }

    // 设置格式
    struct v4l2_format fmt;
    CLEAR(fmt);
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = m_width;
    fmt.fmt.pix.height = m_height;
    fmt.fmt.pix.pixelformat = m_pixelFormat;
    fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;

    if (ioctl(m_v4l2Fd, VIDIOC_S_FMT, &fmt) < 0) {
        emit errorOccurred(QString("VIDIOC_S_FMT failed: %1").arg(strerror(errno)));
        return false;
    }

    // 获取实际设置
    m_width = fmt.fmt.pix.width;
    m_height = fmt.fmt.pix.height;
    m_pixelFormat = fmt.fmt.pix.pixelformat;

    // 申请缓冲区
    struct v4l2_requestbuffers req;
    CLEAR(req);
    req.count = 4;
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;

    if (ioctl(m_v4l2Fd, VIDIOC_REQBUFS, &req) < 0) {
        emit errorOccurred(QString("VIDIOC_REQBUFS failed: %1").arg(strerror(errno)));
        return false;
    }

    m_buffers = new Buffer[req.count];
    m_nBuffers = req.count;

    // 内存映射
    for (unsigned int i = 0; i < req.count; ++i) {
        struct v4l2_buffer buf;
        CLEAR(buf);
        buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        buf.memory = V4L2_MEMORY_MMAP;
        buf.index = i;

        if (ioctl(m_v4l2Fd, VIDIOC_QUERYBUF, &buf) < 0) {
            emit errorOccurred(QString("VIDIOC_QUERYBUF failed: %1").arg(strerror(errno)));
            return false;
        }

        m_buffers[i].length = buf.length;
        m_buffers[i].start = mmap(NULL, buf.length, PROT_READ | PROT_WRITE,
                                 MAP_SHARED, m_v4l2Fd, buf.m.offset);

        if (m_buffers[i].start == MAP_FAILED) {
            emit errorOccurred(QString("mmap failed: %1").arg(strerror(errno)));
            return false;
        }

        if (ioctl(m_v4l2Fd, VIDIOC_QBUF, &buf) < 0) {
            emit errorOccurred(QString("VIDIOC_QBUF failed: %1").arg(strerror(errno)));
            return false;
        }
    }

    // 开始采集
    enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(m_v4l2Fd, VIDIOC_STREAMON, &type) < 0) {
        emit errorOccurred(QString("VIDIOC_STREAMON failed: %1").arg(strerror(errno)));
        return false;
    }

    return true;
}

void CameraThread::cleanup() {
    if (m_v4l2Fd != -1) {
        enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
        ioctl(m_v4l2Fd, VIDIOC_STREAMOFF, &type);

        for (unsigned int i = 0; i < m_nBuffers; ++i) {
            if (m_buffers[i].start != MAP_FAILED) {
                munmap(m_buffers[i].start, m_buffers[i].length);
            }
        }

        close(m_v4l2Fd);
        m_v4l2Fd = -1;
    }

    if (m_buffers) {
        delete[] m_buffers;
        m_buffers = nullptr;
    }
}

QImage CameraThread::convertYUYVToRGB(const unsigned char *yuyv, int width, int height) {
    QImage rgb(width, height, QImage::Format_RGB32);

    for (int y = 0; y < height; ++y) {
        for (int x = 0; x < width; x += 2) {
            int index = y * width * 2 + x * 2;
            unsigned char y0 = yuyv[index];
            unsigned char u = yuyv[index + 1];
            unsigned char y1 = yuyv[index + 2];
            unsigned char v = yuyv[index + 3];

            // 转换YUV到RGB
            auto convert = [](unsigned char y, unsigned char u, unsigned char v) {
                int r = y + 1.402 * (v - 128);
                int g = y - 0.344 * (u - 128) - 0.714 * (v - 128);
                int b = y + 1.772 * (u - 128);

                r = qBound(0, r, 255);
                g = qBound(0, g, 255);
                b = qBound(0, b, 255);

                return qRgb(r, g, b);
            };

            rgb.setPixel(x, y, convert(y0, u, v));
            rgb.setPixel(x + 1, y, convert(y1, u, v));
        }
    }

    return rgb;
}

void CameraThread::run() {
    if (!initCamera()) {
        emit frameReady(QImage());
        return;
    }

    m_running = true;
    struct v4l2_buffer buf;
    CLEAR(buf);
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;

    while (m_running) {
        fd_set fds;
        FD_ZERO(&fds);
        FD_SET(m_v4l2Fd, &fds);

        struct timeval tv = {0};
        tv.tv_sec = 2;

        int r = select(m_v4l2Fd + 1, &fds, NULL, NULL, &tv);
        if (r == -1) {
            if (errno == EINTR) continue;
            emit errorOccurred(QString("select error: %1").arg(strerror(errno)));
            break;
        }

        if (r == 0) {
            emit errorOccurred("Camera timeout");
            continue;
        }

        if (ioctl(m_v4l2Fd, VIDIOC_DQBUF, &buf) < 0) {
            emit errorOccurred(QString("VIDIOC_DQBUF failed: %1").arg(strerror(errno)));
            continue;
        }

        QImage image;
        if (m_pixelFormat == V4L2_PIX_FMT_YUYV) {
            image = convertYUYVToRGB(static_cast<unsigned char*>(m_buffers[buf.index].start),
                                    m_width, m_height);
        } else if (m_pixelFormat == V4L2_PIX_FMT_RGB565) {
            image = QImage(static_cast<unsigned char*>(m_buffers[buf.index].start),
                          m_width, m_height, QImage::Format_RGB16);
        }

        if (!image.isNull()) {
            emit frameReady(image);
        }

        if (ioctl(m_v4l2Fd, VIDIOC_QBUF, &buf) < 0) {
            emit errorOccurred(QString("VIDIOC_QBUF failed: %1").arg(strerror(errno)));
            break;
        }
    }

    cleanup();
}
